﻿using System;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using Compass;

namespace UGSP.NetMF.FEZ.LSM303DLHCompass
{
    public class LSM303DLHCompass
    {
        public enum Unit
        {
            ACC_ADDRESS = (0x30 >> 1),
            MAG_ADDGESS = (0x3C >> 1)
        }
        /* Compass Modes:
         *  Standby: Factory Default.  Measurements are made and the results are returned AFTER the next read command.
         *  Query: Measurements are made and data registers are updated.  A new measurement is started immediately.
         *  Continuous: Continuous measurements are performed and the most recent result are available in the registers.
         * */

        public enum OperationalMode { Continuous = 0, Single = 1, Sleep = 3, }

        /* Output Bias */
        public enum Bias { Normal, Positive, Negative }

        /* Continuous mode frequency:
        *  The frequency of measurements may be set to .75, 1.5, 3, 7.5, 15, 30, or 75 Hz
        */
        public enum Frequency { fp75Hz, f1p5Hz, f3Hz, f7p5Hz, f15Hz, f30Hz, f75Hz }

        /* Gain Modes */
        public enum Gain { g1 = 1, g2, g3, g4, g5, g6, g7 }


        /* Output Data Modes:
         *  Heading: Report 1/10th degrees from 0 to 3599
         *  RawX: ADC Voltage mesurements
         *  RawY: ADC Voltage mesurements
         *  MagnetometerX: Compensated ADC Voltage mesurements
         *  MagnetometerY: Compensated ADC Voltage mesurements
         */
        /*
       public enum OutputMode { Heading = 0, RawX = 1, RawY = 2, MagentometerX = 3, MagentometerY = 4 }
       */
        /* Commands:
         *  w - write to EEPROM
         *  r - read from EEPROM
         *  G - write to RAM
         *  g - read from RAM
         *  S - (Sleep) Enter Sleep Mode
         *  W - (Wake Up) Exit Sleep Mode
         *  O - Update bridge offsets
         *  C - Enter Callibration Mode
         *  E - Exit Callibration Mode
         *  L - Save Operation Mode to RAM
         *  A - Get Data, Compensate and calculate new heading
         */
        /*
        public enum Command
        {
            WriteEEPROM = 0x77, 
            ReadEEPROM = 0x72,
            WriteRAM = 0x47, 
            ReadRAM = 0x67,
            Sleep = 0x53, 
            WakeUp = 0x57, 
            UpdateOffsets = 0x4f,
            StartCallibration = 0x43, 
            EndCallibration = 0x45,
            SaveOperationMode = 0x4c, 
            GetData = 0x41
        }
        */
        /* EEPROM Contents */
        public enum EEPROMAddress
        {
            CRA_REG_M = 0x00,
            CRB_REG_M = 0x01,
            MR_REG_M = 0x02,
            Out_X_H_M = 0x03,
            Out_X_L_M = 0x04,
            Out_Y_H_M = 0x05,
            Out_Y_L_M = 0x06,
            Out_Z_H_M = 0x07,
            Out_Z_L_M = 0x08,
            SR_REG_M = 0x09
        }

        /*
         * Status Register
            RDY - Ready bit. Set when data is written to all six data registers. Cleared when
                the device initiates a write to the data output registers, when in off mode,
                and after one or more of the data output registers are written to. When RDY
                bit is clear, it shall remain cleared for a minimum of 5 μs. The DRDY pin can
                be used as an alternative to the status register for monitoring the device for
                conversion data.
            LOCK - Data output register lock. This bit is set when some, but not all, of the six
                data output registers have been read. When this bit is set, the six data
                output registers are locked and any new data is not placed in these
                registers until one of four conditions are met: one, all six have been read or
                the mode changed, two, a POR is issued, three, the mode is changed, or
                four, the measurement is changed.
            REN - Regulator enabled bit. This bit is set when the internal voltage regulator is
                enabled. This bit is cleared when the internal regulator is disabled.
        */
        public enum Status
        {
            Ready,
            Lock,
            RegulatorEnable
        }


        I2CDevice.Configuration myDeviceConfiguration;
        I2CDevice myCompass;
        I2CDevice.I2CTransaction[] myI2Command = new I2CDevice.I2CTransaction[2];

        byte[] myReadBuffer = new byte[6];
        int myBytesTransmitted = 0;

        // this constructor assumes the default Magnetic Slave Address of 0x3C

        public LSM303DLHCompass()
        {
            myDeviceConfiguration = new I2CDevice.Configuration(0x1E, 100);
            myCompass = new I2CDevice(myDeviceConfiguration);
        }

        // This constructor allows user to specify the Slave Address

        public LSM303DLHCompass(byte addr)
        {
            byte target = (byte)(addr >> 1);
            myDeviceConfiguration = new I2CDevice.Configuration(target, 100);
            myCompass = new I2CDevice(myDeviceConfiguration);
        }

        public float GetHeading()
        {
            myI2Command[0] = I2CDevice.CreateWriteTransaction(new byte[] { (byte)Unit.MAG_ADDGESS, (byte)EEPROMAddress.Out_X_H_M });
            myI2Command[1] = I2CDevice.CreateReadTransaction(myReadBuffer);
            myBytesTransmitted = myCompass.Execute(myI2Command, 100);
            float heading = ((myReadBuffer[0] << 8) + myReadBuffer[1]) / 10f;
            return (heading);
        }

        // After starting Callibration, the user should rotate the sensor through 2 or more complete rotations.
        // The optimal time is 2 rotations over 20 seconds.
        /*
        public void StartCalibration()
        {
            myI2Command[0] = I2CDevice.CreateWriteTransaction(new byte[] { (byte)Command.StartCallibration });
            myBytesTransmitted = myCompass.Execute(myI2Command, 100);
        }

        // Call this method to end the callibration Sequence.

        public void EndCallibration()
        {
            myI2Command[0] = I2CDevice.CreateWriteTransaction(new byte[] { (byte)Command.EndCallibration });
            myBytesTransmitted = myCompass.Execute(myI2Command, 100);
        }
        */

        /*
         * Only reads from the MAG registers
         */
        public byte ReadEeprom(Unit u, EEPROMAddress addr)
        {
            myI2Command[0] = I2CDevice.CreateWriteTransaction(new byte[] { (byte)u, (byte)addr });
            myI2Command[1] = I2CDevice.CreateReadTransaction(myReadBuffer);
            myBytesTransmitted = myCompass.Execute(myI2Command, 100);
            return myReadBuffer[0];


        }

        /* There is no generic public Write EEPROM command. 
         * Specific commands are available for setting EEPROM values
         * where appropriate to the device. */

        void WriteEeprom(Unit u, EEPROMAddress addr, byte data)
        {
            myI2Command[0] = I2CDevice.CreateWriteTransaction(new byte[] { (byte)u, (byte)addr, data });
            myBytesTransmitted = myCompass.Execute(myI2Command, 100);
        }


        /* 
         * For Accelerometer (potentially)
         */
        /*
        public void Sleep()
        {
            myI2Command[0] = I2CDevice.CreateWriteTransaction(new byte[] { (byte)Command.Sleep });
            myBytesTransmitted = myCompass.Execute(myI2Command, 100);
        }

        public void WakeUp()
        {
            myI2Command[0] = I2CDevice.CreateWriteTransaction(new byte[] { (byte)Command.WakeUp });
            myBytesTransmitted = myCompass.Execute(myI2Command, 100);
        }
        */

        /*
         * Sets Operational Mode manually
         * Requires Bias/Frequency (CRA_REG_M), Gain (CRB_REG_M), Mode (MR_REG_M)
         */

        public void SetOperationalMode(Bias bias, OperationalMode mode, Frequency freq, Gain gain)
        {
            byte b = (byte)((byte)bias);
            byte f = (byte)((byte)freq << 2);
            byte op = (byte)((byte)b | f);

            byte g = (byte)((byte)gain << 5);

            byte m = (byte)((byte)mode);

            WriteEeprom(Unit.MAG_ADDGESS, EEPROMAddress.CRA_REG_M, op);
            WriteEeprom(Unit.MAG_ADDGESS, EEPROMAddress.CRB_REG_M, g);
            WriteEeprom(Unit.MAG_ADDGESS, EEPROMAddress.MR_REG_M, m);
        }

        /*
         * StandardSetp 
         * +/- 1.3 Gauss
         * 30Hz Continuous Polling
         */
        public void StandardSetup()
        {
            WriteEeprom(Unit.MAG_ADDGESS, EEPROMAddress.CRA_REG_M, 0x14);
            WriteEeprom(Unit.MAG_ADDGESS, EEPROMAddress.MR_REG_M, 0x00);
        }
        /*
        public OperationalMode GetOperationalMode()
        {
            byte mode = ReadEeprom(EEPROMAddress.OperationalModeByte);
            return (OperationalMode)(mode & 0x02);
        }

        public Frequency GetFrequency()
        {
            byte freq = ReadEeprom(EEPROMAddress.OperationalModeByte);
            return (Frequency)(freq >> 5);
        }
        */

        /*
        int Heading()
        {
            Vector from = new Vector (0,-1,0);
            return Heading(from);
        }

        // Returns the number of degrees from the from vector that it
        // is pointing.

        int Heading(Vector from)
        {
            // shift and scale
            m.x = (m.x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0;
            m.y = (m.y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0;
            m.z = (m.z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0;

            Vector temp_a = a;
            // normalize
            Vector_normalize(&temp_a);
            //vector_normalize(&m);

            // compute E and N
            Vector E;
            Vector N;
            Vector_cross(&m, &temp_a, &E);
            Vector_normalize(&E);
            Vector_cross(&temp_a, &E, &N);

            // compute heading
            int heading = System.Math.Round(Microsoft.SPOT.Math.atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / System.Math.PI);
            if (heading < 0) heading += 360;

            return heading;
        }
        */
    }
    public class Vector
    {
        float x, y, z;

        public Vector()
        {
            x = 0;
            y = 0;
            z = 0;
        }

        public Vector(float a, float b, float c)
        {
            x = a;
            y = b;
            z = c;
        }

        public float Vector_dot(Vector a, Vector b)
        {
            float dot = a.x * b.x + a.y * b.y + a.z * b.z;
            return dot;
        }

        public Vector Vector_cross(Vector a, Vector b)
        {
            Vector cross = new Vector();

            cross.x = a.y * b.z - a.z * b.y;
            cross.y = a.z * b.x - a.x * b.z;
            cross.z = a.x * b.y - a.y * b.x;

            return cross;
        }

    }

    public class Heading
    {
        public int heading;
    }
}